#pragma once
#include "elisa/elisa.h"
#include <iostream>
#include <sstream>
#include <algorithm>
#include <string>
#include <vector>
#include <gaol/gaol>

#include "Modeler.h"

#ifdef ELISA_PERFORMER
#include "elisa_chrono.h"
#endif

using namespace std;
using namespace elisa;

/* Problem: kinematics 1

@book{Numerica,
author    = {P. {Van Hentenryck} and L. Michel and Y. Deville},
title     = {{N}umerica: a {M}odeling {L}anguage for {G}lobal {O}ptimization},
publisher = {MIT Press},
year      = {1997}
}
*/
class Kinematics1 : public Modeler
{
private:
	Model _model;

	RealConstant alpha1;
	RealConstant alpha2;
	RealConstant alpha3;
	RealConstant alpha4;
	RealConstant alpha5;
	RealConstant alpha6;
	RealVar s1, s2, s3, s4, s5, s6;
	RealVar c1, c2, c3, c4, c5, c6;
	RealVar a1, a2, a3, a4;
	RealConstraint ct1;
	RealConstraint ct2;
	RealConstraint ct3;
	RealConstraint ct4;
	RealConstraint ct5;
	RealConstraint ct6;
	RealConstraint ct7;
	RealConstraint ct8;
	RealConstraint ct9;
	RealConstraint ct10;
	RealConstraint ct11;
	RealConstraint ct12;
	RealConstraint ct13;
	RealConstraint ct14;
	RealConstraint ct15;
	RealConstraint ct16;

public:
	Kinematics1()
		: s1(Interval(-1,1), "s1"),
		  s2(Interval(-1,1), "s2"),
		  s3(Interval(-1,1), "s3"),
		  s4(Interval(-1,1), "s4"),
		  s5(Interval(-1,1), "s5"),
		  s6(Interval(-1,1), "s6"),
		  c1(Interval(-1,1), "c1"),
		  c2(Interval(-1,1), "c2"),
		  c3(Interval(-1,1), "c3"),
		  c4(Interval(-1,1), "c4"),
		  c5(Interval(-1,1), "c5"),
		  c6(Interval(-1,1), "c6"),
		  a1(Interval::Universe, "a1"),
		  a2(Interval::Universe, "a2"),
		  a3(Interval::Universe, "a3"),
		  a4(Interval::Universe, "a4"),
		  alpha1(3.9701),
		  alpha2(4.0616),
		  alpha3(1.7172),
		  alpha4(1.9791),
		  alpha5(0.4077),
		  alpha6(1.9115),
		  ct1(1 == pow(s1,2) + pow(c1,2)),
		  ct2(1 == pow(s2,2) + pow(c2,2)),
		  ct3(1 == pow(s3,2) + pow(c3,2)),
		  ct4(1 == pow(s4,2) + pow(c4,2)),
		  ct5(1 == pow(s5,2) + pow(c5,2)),
		  ct6(1 == pow(s6,2) + pow(c6,2)),
		  ct7(alpha1 == 3*s2 + 2*s3 + s4),
		  ct8(alpha2 == c1*a4),
		  ct9(alpha3 == s1*a4),
		  ct10(alpha4 == s5*a3),
		  ct11(alpha5 == c6*a2 - c5*s6*a3),
		  ct12(alpha6 == c1*s5*a2 + s1*c5),
		  ct13(a1 == s2 -s3 -s4),
		  ct14(a2 == c2 + c3 + c4),
		  ct15(a3 == s2 + s3 + s4),
		  ct16(a4 == 3*c2 + 2*c3 + c4),
		  _model()
	{
	}

	elisa::Model & model()
	{
		_model = ct1 && ct2 && ct3 && ct4 && ct5 && ct6 && ct7 && ct8
			&& ct9 && ct10 && ct11 && ct12 && ct13 && ct14 && ct15 && ct16;
		return _model;
	}

	void print()
	{
		cout << '\t' << s1.getName() << " = " << s1.domain() << endl;
		cout << '\t' << s2.getName() << " = " << s2.domain() << endl;
		cout << '\t' << s3.getName() << " = " << s3.domain() << endl;
		cout << '\t' << s4.getName() << " = " << s4.domain() << endl;
		cout << '\t' << s5.getName() << " = " << s5.domain() << endl;
		cout << '\t' << s6.getName() << " = " << s6.domain() << endl;
		cout << '\t' << c1.getName() << " = " << c1.domain() << endl;
		cout << '\t' << c2.getName() << " = " << c2.domain() << endl;
		cout << '\t' << c3.getName() << " = " << c3.domain() << endl;
		cout << '\t' << c4.getName() << " = " << c4.domain() << endl;
		cout << '\t' << c5.getName() << " = " << c5.domain() << endl;
		cout << '\t' << c6.getName() << " = " << c6.domain() << endl;
	}

	RealVar & varX()
	{
		return s1;
	}
	RealVar & varY()
	{
		return c2;
	}
};
